/**
	*****************************************
	* @file Shoot_task.c
	* @author Chensy
	* @brief 发射运行任务
	* @version 1.0
	* @date 2025-03-02
	* 
	* @copyright Copyright (c) 2025
	*
	******************************************
 **/	
 
#include "Shoot_task.h"


remoter_t *shoot_rc;  
#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t shoot_high_water;
#endif

/**
  * @brief 发射任务（5ms周期）
  * @param argument: 空
  * @retval 无
  */
void Shoot_Task(void *argument)
{       
		vTaskDelay(5);
    Shoot_motor_init();
		while(1)
		{
			
			switch(g_system_state.control_mode)
			{
					case MODE_NONE:
					break;
					
					case MODE_LAUNCH:
					if(g_system_state.operate_mode == OPERATE_AUTO){
								 //unitree_rc_ctrl(GO_motor,&huart1,&UNITREE_motor_s,&UNITREE_motor_r,shoot_rc);
						
					}	
					else if(g_system_state.operate_mode == OPERATE_MANUAL1 || g_system_state.operate_mode == OPERATE_MANUAL2){
                                //Manual_launch();
								//Shoot_Manual_start();	
                                unitree_rc_ctrl(GO_motor,&huart1,&UNITREE_motor_s,&UNITREE_motor_r,shoot_rc);
                        
					}
					break;
					
					case MODE_ARM:
					break;
					
					default:
					break;


			}
				osDelay(10);
#if INCLUDE_uxTaskGetStackHighWaterMark
        shoot_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
		}
}
		
void Shoot_motor_init(void)
{
			//激活好盈电调
			 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
     __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,PWM_Init_low);  
			osDelay(1000);	
			
			Pitch_init(&UNITREE_motor_s);
			shoot_rc = get_remote_control_point();
}




//手动模式操作
//看看哪种操作方式合适
//1.SWB和SWC直接赋值电机转速，然后SWD只控制电推杆

																//					|			手动1 		|
			
static uint16_t speed_pwm[SPEED_LEVELS] = {1000, 1300, 1500,
	
																//					|     手动2			|					
																					 1700, 1870, MAX_PWM};
void Manual_launch(void)
{
		if(g_system_state.operate_mode == OPERATE_MANUAL1){
				switch (g_system_state.function_mode)
				{
					case FUNCTION_MANUAL1_OP1:
					__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_pwm[0]);		
					break;
					
					case FUNCTION_MANUAL1_OP2:
					__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_pwm[1] );
					break;
					
					case FUNCTION_MANUAL1_OP3:
					__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_pwm[2]);
					break;
					
					default:
					break;
				}
		
		}	
		else if(g_system_state.operate_mode == OPERATE_MANUAL2){
				switch (g_system_state.function_mode)
				{
					case FUNCTION_MANUAL2_OP1:
					__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_pwm[5]);		
					break;
					
					case FUNCTION_MANUAL2_OP2:
					__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_pwm[4] );
					break;
					
					case FUNCTION_MANUAL2_OP3:
					__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_pwm[3]);
					break;
					
					default:
					break;
				}
		
		}
		
}


void Shoot_Manual_start(void)
{
		//发射开关
    //双路继电器，一个gpio口输出高低电平，控制气缸推杆
    if(g_system_state.control_state == CONTROL_ENABLE){
        HAL_GPIO_WritePin(IN1_GPIO_Port,IN1_Pin,GPIO_PIN_RESET);
		}else if(g_system_state.control_state == CONTROL_DISABLE){
        HAL_GPIO_WritePin(IN1_GPIO_Port,IN1_Pin,GPIO_PIN_SET);
    }
		
		
}

//2.SWB和SWC是来赋值，然后SWD是启动和关闭电机转动、电推杆；
/*
uint16 Speed_PWM=0;
void Manual_launch(void)
{
		if(g_system_state.operate_mode == OPERATE_MANUAL1){
				switch (g_system_state.function_mode)
				{
					case FUNCTION_MANUAL1_OP1:
					Speed_PWM = 1100;		
					break;
					
					case FUNCTION_MANUAL1_OP2:
					Speed_PWM = 1300;
					break;
					
					case FUNCTION_MANUAL1_OP3:
					Speed_PWM = 1500;
					break;
					
					default:
					break;
				}
		
		}	
		else if(g_system_state.operate_mode == OPERATE_MANUAL2){
				switch (g_system_state.function_mode)
				{
					case FUNCTION_MANUAL2_OP1:
					Speed_PWM = 1940;		
					break;
					
					case FUNCTION_MANUAL2_OP2:
					Speed_PWM = 1800;
					break;
					
					case FUNCTION_MANUAL2_OP3:
					Speed_PWM = 1700;
					break;
					
					default:
					break;
				}
		
		}
		
}
void Shoot_Manual_start(void)
{
		//发射开关
		if(g_system_state.control_state == CONTROL_ENABLE){
				//开启电机
				__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, Speed_PWM);
				osDelay(10);
				
		}else if(g_system_state.control_state == CONTROL_DISABLE){
				//关闭电机
				__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 1100);
		}
		
}
*/

void Pitch_init(UNITREE_MOTOR_send *unitree_motor_init)
{
	unitree_motor_init->id=pitch_motor_id;
	unitree_motor_init->K_P=0;
	unitree_motor_init->K_W=0.02;
	unitree_motor_init->mode=go_start;
// ? ?unitree_motor_init->Pos=0;
	unitree_motor_init->T=0;
	unitree_motor_init->Speed=0;
	unitree_uart_Send_recv(GO_motor,&huart1,unitree_motor_init,&UNITREE_motor_r);
	Init_Pos = UNITREE_motor_r.Pos;
	Min_Pos = Init_Pos;
}


/**
  * @brief 发射调参，向vofa发送数据
  * @param 无
  * @retval 无
  */
void vofa_shoot_data(void)
{
		

}
